/*
 * @brief Sensor Hub demo teapot (implementation file)
 *
 * @note
 * Copyright(C) NXP Semiconductors, 2015
 * All rights reserved.
 *
 * @par
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * LPC products.  This software is supplied "AS IS" without any warranties of
 * any kind, and NXP Semiconductors and its licensor disclaim any and
 * all warranties, express or implied, including all implied warranties of
 * merchantability, fitness for a particular purpose and non-infringement of
 * intellectual property rights.  NXP Semiconductors assumes no responsibility
 * or liability for the use of the software, conveys no license or rights under any
 * patent, copyright, mask work right, or any other intellectual property rights in
 * or to any products. NXP Semiconductors reserves the right to make changes
 * in the software without notification. NXP Semiconductors also makes no
 * representation or warranty that such application will be suitable for the
 * specified use without further testing or modification.
 *
 * @par
 * Permission to use, copy, modify, and distribute this software and its
 * documentation is hereby granted, under NXP Semiconductors' and its
 * licensor's relevant copyrights in the software, without fee, provided that it
 * is used in conjunction with NXP Semiconductors microcontrollers.  This
 * copyright, permission, and disclaimer notice must appear in all copies of
 * this code.
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <math.h>

#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

#include "hostif_protocol.h"

#include "trace.h"
#include "gl_menu.h"
#include "gl_text.h"
#include "misc.h"
#include "teapot.h"

/*****************************************************************************
 * Public types/enumerations/variables
 ****************************************************************************/

/*****************************************************************************
 * Private types/enumerations/variables
 ****************************************************************************/
static	float	rotationMatrix[16] = {										/* rotation matrix */
	1.0, 0, 0, 0,
	0, 1.0, 0, 0,
	0, 0, 1.0, 0,
	0, 0, 0, 1.0
};
static	float	offsetMatrix[16] = {											/* translation matrix */
	1.0, 0, 0, 0,
	0, 1.0, 0, 0,
	0, 0, 1.0, 0,
	0, 0, 0, 1.0
};
static 	float	inverse[4][4];
static	bool	updateOffset = false;

static	uint8_t		id;
static	uint32_t	ts;

/*****************************************************************************
 * Private functions
 ****************************************************************************/

/**
 * @brief: For calculating Determinant of the Matrix
 * @return	Nothing
 */
static float determinant(float a[4][4], float k)
{
	float s=1, det=0, b[4][4];
	int i,j,m,n,c;

	if (k == 1) {
		return (a[0][0]);
	} else {
		for (c=0; c<k; c++) {
			m=0, n=0;
			for (i=0; i<k; i++) {
				for (j=0; j<k; j++) {
					b[i][j]=0;
					if (i != 0 && j != c) {
						b[m][n]=a[i][j];
						if (n<(k-2))
							n++;
						else {
							n=0;
							m++;
						}
					}
				}
			}
			det = det + s * (a[0][c] * determinant(b,k-1));
			s = -1 * s;
		}
	}

	return (det);
}

/**
 * @brief Calculate matrix transposition
 * @return	Nothing
 */
static void transpose(float num[4][4], float fac[4][4], float r)
{
	int i,j;
	float b[4][4],d;

	for (i=0; i<r; i++) {
		for (j=0; j<r; j++) {
			b[i][j]=fac[j][i];
		}
	}
	d=determinant(num,r);
	for (i=0; i<r; i++) {
		for (j=0; j<r; j++) {
			inverse[i][j]=b[i][j] / d;
		}
	}
}


/**
 * @brief
 * @return	Nothing
 */
static void cofactor(float num[4][4], float f)
{
	float b[4][4], fac[4][4];
	int p, q, m, n, i, j;

	for (q=0; q<f; q++) {
		for (p=0; p<f; p++) {
			m=0, n=0;
			for (i=0; i<f; i++) {
				for (j=0; j<f; j++) {
					if (i != q && j != p) {
						b[m][n]=num[i][j];
						if (n<(f-2))
							n++;
						else {
							n=0;
							m++;
						}
					}
				}
			}
			fac[q][p]=pow(-1,q + p) * determinant(b,f-1);
		}
	}
	transpose(num,fac,f);
}

/**
 * @brief
 * @return	Nothing
 */
static void invert(float matrix[][4])
{
	if (determinant(matrix,4) != 0) {
		cofactor(matrix,4);
	}
}


/**
 * @brief
 * @return	Nothing
 */
static void init_material(void)
{
	/* define material properties */
	GLfloat material_ambient[] = { 0.25f, 0.25f, 0.25f };
	GLfloat material_diffuse[] = { 0.90f, 0.90f, 0.90f };
	GLfloat material_specular[] = { 0.90f, 0.90f, 0.90f };
	GLfloat material_shininess = 25.0f;

	/* load material properties */
	glMaterialfv(GL_FRONT, GL_AMBIENT, material_ambient);
	glMaterialfv(GL_FRONT, GL_DIFFUSE, material_diffuse);
	glMaterialfv(GL_FRONT, GL_SPECULAR, material_specular);
	glMaterialf(GL_FRONT, GL_SHININESS, material_shininess);
}

/**
 * @brief
 * @return	Nothing
 */
static void init_lights(void)
{
	/* define light properties */
	GLfloat light0_diffuse[] = { 1.0, 0.0, 0.0, 1.0 };
	GLfloat light0_position[] = { -1.0, 0.0, 1.0, 0.0 };
	GLfloat light1_diffuse[] = { 0.0, 0.0, 1.0, 1.0 };
	GLfloat light1_position[] = { 1.0, 0.0, 1.0, 0.0 };

	/* enable lights 0 and 1*/
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHT1);

	/* load light properties */
	glLightfv(GL_LIGHT0, GL_DIFFUSE, light0_diffuse);
	glLightfv(GL_LIGHT0, GL_POSITION, light0_position);
	glLightfv(GL_LIGHT1, GL_DIFFUSE, light1_diffuse);	
	glLightfv(GL_LIGHT1, GL_POSITION, light1_position);
}

/**
 * @brief
 * @return	Nothing
 */
static void update_rotation_matrix(int16_t* rotationVector)
{
	int i, j;

	/* Convert the quaternion data from fixed point to float */
	float q0 = (float)rotationVector[0] / 16384.0f;
	float q1 = (float)rotationVector[2] / 16384.0f;
	float q2 = (float)rotationVector[3] / 16384.0f;
	float q3 = (float)rotationVector[1] / 16384.0f;

	float sq_q1 = 2 * q1 * q1;
	float sq_q2 = 2 * q2 * q2;
	float sq_q3 = 2 * q3 * q3;
	float q1_q2 = 2 * q1 * q2;
	float q3_q0 = 2 * q3 * q0;
	float q1_q3 = 2 * q1 * q3;
	float q2_q0 = 2 * q2 * q0;
	float q2_q3 = 2 * q2 * q3;
	float q1_q0 = 2 * q1 * q0;

	rotationMatrix[0] = 1 - sq_q2 - sq_q3;
	rotationMatrix[1] = q1_q2 - q3_q0;
	rotationMatrix[2] = q1_q3 + q2_q0;
	rotationMatrix[3] = 0.0f;

	rotationMatrix[4] = q1_q2 + q3_q0;
	rotationMatrix[5] = 1 - sq_q1 - sq_q3;
	rotationMatrix[6] = q2_q3 - q1_q0;
	rotationMatrix[7] = 0.0f;

	rotationMatrix[8] = q1_q3 - q2_q0;
	rotationMatrix[9] = q2_q3 + q1_q0;
	rotationMatrix[10] = 1 - sq_q1 - sq_q2;
	rotationMatrix[11] = 0.0f;

	rotationMatrix[12] = rotationMatrix[13] = rotationMatrix[14] = 0.0f;
	rotationMatrix[15] = 1.0f;

	if (updateOffset == true) {
		updateOffset = false;
		invert((float (*)[4])rotationMatrix);
		for(i = 0; i < 4; i++) {
			for(j = 0; j < 4; j++) {
				offsetMatrix[i*4 + j] = inverse[i][j];
			}
		}
	}
}

/*****************************************************************************
 * Public functions
 ****************************************************************************/

/**
 * @brief
 * @return	Nothing
 */
void teapot_init(void)
{
	glClearColor(0.0, 0.0, 0.0, 1.0f);											/* set the clear color (R, G, B, A) */
	glEnable(GL_DEPTH_TEST);													/* enable depth testing */
	glEnable(GL_LIGHTING);														/* enabled lighting */
	init_material();															/* initialize materials */
	init_lights();																/* initialize lights */
	gluLookAt(0.0, 0.0, 10.0,			/* eye point */							/* position and orient the camera */
	          0.0, 0.0, 0.0,			/* reference point */
	          0.0, 1.0, 0.0);			/* up vector */
}

/**
 * @brief
 * @return	Nothing
 */
void teapot_data_acq(uint8_t* rx)
{
	int16_t		vector[4];
	struct		lpcsh_sensor_node* pData;
	pData = (struct lpcsh_sensor_node*)rx;

	id = pData->header.sensorId;
	ts = pData->header.timeStamp;

	memcpy(vector, &(pData->data.quaternionData), sizeof(vector));
	vector[1] = -vector[1];
	vector[2] = -vector[2];
	vector[3] = -vector[3];
	update_rotation_matrix(vector);
}

/**
 * @brief
 * @return	Nothing
 */
void teapot_update_offset(void)
{
	updateOffset = true;
}

/**
 * @brief
 * @return	Nothing
 */
void teapot_update_gl(void)
{
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
	glTranslatef(0, 0, -8.0f);
	glMultMatrixf(offsetMatrix);
	glMultMatrixf(rotationMatrix);
}
